/**

          _____                    _____                    _____                _____
         /\    \                  /\    \                  /\    \              /\    \
        /::\    \                /::\    \                /::\    \            /::\    \
       /::::\    \               \:::\    \              /::::\    \           \:::\    \
      /::::::\    \               \:::\    \            /::::::\    \           \:::\    \
     /:::/\:::\    \               \:::\    \          /:::/\:::\    \           \:::\    \
    /:::/  \:::\    \               \:::\    \        /:::/__\:::\    \           \:::\    \
   /:::/    \:::\    \              /::::\    \      /::::\   \:::\    \          /::::\    \
  /:::/    / \:::\    \    ____    /::::::\    \    /::::::\   \:::\    \        /::::::\    \
 /:::/    /   \:::\ ___\  /\   \  /:::/\:::\    \  /:::/\:::\   \:::\____\      /:::/\:::\    \
/:::/____/  ___\:::|    |/::\   \/:::/  \:::\____\/:::/  \:::\   \:::|    |    /:::/  \:::\____\
\:::\    \ /\  /:::|____|\:::\  /:::/    \::/    /\::/   |::::\  /:::|____|   /:::/    \::/    /
 \:::\    /::\ \::/    /  \:::\/:::/    / \/____/  \/____|:::::\/:::/    /   /:::/    / \/____/
  \:::\   \:::\ \/____/    \::::::/    /                 |:::::::::/    /   /:::/    /
   \:::\   \:::\____\       \::::/____/                  |::|\::::/    /   /:::/    /
    \:::\  /:::/    /        \:::\    \                  |::| \::/____/    \::/    /
     \:::\/:::/    /          \:::\    \                 |::|  ~|           \/____/
      \::::::/    /            \:::\    \                |::|   |
       \::::/    /              \:::\____\               \::|   |
        \::/____/                \::/    /                \:|   |
                                  \/____/                  \|___|
//桂林信息科技学院GIRT机器人队
  ****************************(C) COPYRIGHT 2023 GIRT****************************
  * @file       can_receive.c/h
  * @brief      there is CAN interrupt function  to receive motor data,
  *             and CAN send function to send motor current to control motor.
  *             这里是CAN中断接收函数，接收电机数据,CAN发送函数发送电机电流控制电机.
  * @note       
  * @history
  *  Version    Date            Author          Modification
  *  V1.0.0     Dec-26-2018     RM              1. done
  *  V1.1.0     Nov-11-2019     RM              1. support hal lib
  *  V1.2.0     March-17-2023   HZY             1. Fit the 23 season suction cup engineering car
  *
  @verbatim
  ==============================================================================

  ==============================================================================
  @endverbatim
  ****************************(C) COPYRIGHT 2023 GIRT****************************
  */

#include "CAN_receive.h"

//#include "cmsis_os.h"

#include "main.h"
//#include "bsp_rng.h"
//#include "user_lib.h"

//#include "detect_task.h"

extern CAN_HandleTypeDef hcan;


uint8_t State_Info[8];     //用于存放副控板发送的数据

//motor data read
#define get_motor_measure(ptr, data)                                    \
    {                                                                   \
        (ptr)->last_ecd = (ptr)->ecd;                                   \
        (ptr)->ecd = (uint16_t)((data)[0] << 8 | (data)[1]);            \
        (ptr)->speed_rpm = (uint16_t)((data)[2] << 8 | (data)[3]);      \
        (ptr)->given_current = (uint16_t)((data)[4] << 8 | (data)[5]);  \
        (ptr)->temperate = (data)[6];                                   \
    }
#define get_dm_motor_measure(ptr, data)                                 \
    {                                                                   \
        (ptr)->id = (data)[0] & 0x0F;                                   \
        (ptr)->err = (data)[0] >> 4;                                    \
        (ptr)->p_int = ((data)[1] << 8 | (data)[2]);                    \
        (ptr)->v_int = ((data)[3] << 4 | (data)[4] >> 4);               \
        (ptr)->t_int = (((data)[4] & 0xF) << 8 | (data)[5]);            \
        (ptr)->position = uint_to_float((ptr)->p_int, P_MIN, P_MAX, 16);\
        (ptr)->velocity = uint_to_float((ptr)->v_int, V_MIN, V_MAX, 12);\
        (ptr)->torque = uint_to_float((ptr)->t_int, T_MIN, T_MAX, 12);  \
        (ptr)->T_MOS = (float)(data[6]);                                \
        (ptr)->T_Rotor = (float)(data[7]);                              \
    }

static dm_measure_t dm_chassis[3];    		  //用于存放CAN1总线电机反馈数据的数组
static motor_measure_t motor_gimbal[7];     //用于存放CAN2总线电机反馈数据的数组


static CAN_TxHeaderTypeDef  chassis_tx_message;
static uint8_t              chassis_can_send_data[8];
static CAN_TxHeaderTypeDef  gimbal_tx_message;
static uint8_t              gimbal_can_send_data[8];

/**
 * @brief  uint转float
 * @param  x_int   最大值
 * @param  x_min   最小值
 * @param  bits    位数
 */
float uint_to_float(int x_int, float x_min, float x_max, int bits)
{
  float span = x_max - x_min;
  float offset = x_min;
  return ((float)x_int)*span/((float)((1<<bits)-1)) + offset;
}		
		
/**
  * @brief          hal CAN fifo call back, receive motor data
  * @param[in]      hcan, the point to CAN handle
  * @retval         none
  */
/**
  * @brief          hal库CAN回调函数,接收电机数据
  * @param[in]      hcan:CAN句柄指针
  * @retval         none
  */
void HAL_CAN_RxFifo0MsgPendingCallback(CAN_HandleTypeDef *hcan)
{
  CAN_RxHeaderTypeDef rx_header;
  uint8_t rx_data[8];

  HAL_CAN_GetRxMessage(hcan, CAN_RX_FIFO0, &rx_header, rx_data);

//  if (hcan->Instance == CAN1)
//  {
    switch (rx_header.StdId)
    {
		case SUCKER_YAW_MOTOR_MATER_ID:
		case SUCKER_PITCH_F_MOTOR_MATER_ID:
		case SUCKER_PITCH_B_MOTOR_MATER_ID:
		{
			static uint8_t i = 0;
			//get motor id
			i = rx_header.StdId - SUCKER_YAW_MOTOR_MATER_ID;
			get_dm_motor_measure(&dm_chassis[i], rx_data);
			break;
		}
      default:
      {
          break;
      }
    }
//  }
//  if (hcan->Instance == CAN)
//  {
    switch (rx_header.StdId)
    {
      case CAN_SIDESWAY_MOTOR_ID:
			case CAN_SUCKER_ROLL_MOTOR_ID:
      case CAN_TELESCOPIC_EX_ML_ID:
      case CAN_TELESCOPIC_EX_MR_ID:
			case CAN_RESCUE_ML_ID:
			case CAN_RESCUE_MR_ID:
      {
        static uint8_t i = 0;
        //get motor id
        i = rx_header.StdId - CAN_SIDESWAY_MOTOR_ID;
        get_motor_measure(&motor_gimbal[i], rx_data);
        // detect_hook(CAN_LEFT_PRIMARY_TELESCOPIC_MOTOR_ID + i);     //错误码，还没写
        break;
      }
      case CAN_INFO_MASTER_ID:
        {
          static uint8_t i = 0;
          for (i = 0; i < 8; i++)
          {
            State_Info[i] = rx_data[i];
          }
        }
        break;
      default:
      {
          break;
      }
    }
//  }
}



/**
 * @brief  float转uint
 * @param  x_int   最大值
 * @param  x_min   最小值
 * @param  bits    位数
 */
int float_to_uint(float x, float x_min, float x_max, int bits)
{ 
  float span = x_max - x_min;
  float offset = x_min;
  return (int) ((x-offset)*((float)((1<<bits)-1))/span);
}

/**
 * @brief 		电机出现过热等错误时，发送“清除”命令可以清除错误。
 * @param[in] can id 
 * @retval 		none
 */
void clear_errors_J4310motor(uint16_t id)
{
  uint32_t send_mail_box;
  chassis_tx_message.StdId = id;
  chassis_tx_message.IDE = CAN_ID_STD;
  chassis_tx_message.RTR = CAN_RTR_DATA;
  chassis_tx_message.DLC = 0x08;
  chassis_can_send_data[0] = 0xFF;
  chassis_can_send_data[1] = 0xFF;
  chassis_can_send_data[2] = 0xFF;
  chassis_can_send_data[3] = 0xFF;
  chassis_can_send_data[4] = 0xFF;
  chassis_can_send_data[5] = 0xFF;
  chassis_can_send_data[6] = 0xFF;
  chassis_can_send_data[7] = 0xFB;
  HAL_CAN_AddTxMessage(&hcan, &chassis_tx_message, chassis_can_send_data, &send_mail_box);
}

/**
 * @brief 		使能J4310电机
 * @param[in] can id 
 * @retval 		none
 */
void start_J4310motor(uint16_t id)
{
  uint32_t send_mail_box;
  chassis_tx_message.StdId = id;
  chassis_tx_message.IDE = CAN_ID_STD;
  chassis_tx_message.RTR = CAN_RTR_DATA;
  chassis_tx_message.DLC = 0x08;
  chassis_can_send_data[0] = 0xFF;
  chassis_can_send_data[1] = 0xFF;
  chassis_can_send_data[2] = 0xFF;
  chassis_can_send_data[3] = 0xFF;
  chassis_can_send_data[4] = 0xFF;
  chassis_can_send_data[5] = 0xFF;
  chassis_can_send_data[6] = 0xFF;
  chassis_can_send_data[7] = 0xFC;
  HAL_CAN_AddTxMessage(&hcan, &chassis_tx_message, chassis_can_send_data, &send_mail_box);
}

/**
 * @brief 		失能J4310电机
 * @param[in] can id 
 * @retval 		none
 */
void lock_J4310motor(uint16_t id)
{
  uint32_t send_mail_box;
  chassis_tx_message.StdId = id;
  chassis_tx_message.IDE = CAN_ID_STD;
  chassis_tx_message.RTR = CAN_RTR_DATA;
  chassis_tx_message.DLC = 0x08;
  chassis_can_send_data[0] = 0xFF;
  chassis_can_send_data[1] = 0xFF;
  chassis_can_send_data[2] = 0xFF;
  chassis_can_send_data[3] = 0xFF;
  chassis_can_send_data[4] = 0xFF;
  chassis_can_send_data[5] = 0xFF;
  chassis_can_send_data[6] = 0xFF;
  chassis_can_send_data[7] = 0xFD;
  HAL_CAN_AddTxMessage(&hcan, &chassis_tx_message, chassis_can_send_data, &send_mail_box);
}

/**
 * @brief 		保存J4310电机零点
 * @param[in] can id 
 * @retval 		none
 */
void save_zero_J4310motor(uint16_t id)
{
  uint32_t send_mail_box;
  chassis_tx_message.StdId = id;
  chassis_tx_message.IDE = CAN_ID_STD;
  chassis_tx_message.RTR = CAN_RTR_DATA;
  chassis_tx_message.DLC = 0x08;
  chassis_can_send_data[0] = 0xFF;
  chassis_can_send_data[1] = 0xFF;
  chassis_can_send_data[2] = 0xFF;
  chassis_can_send_data[3] = 0xFF;
  chassis_can_send_data[4] = 0xFF;
  chassis_can_send_data[5] = 0xFF;
  chassis_can_send_data[6] = 0xFF;
  chassis_can_send_data[7] = 0xFE;
  HAL_CAN_AddTxMessage(&hcan, &chassis_tx_message, chassis_can_send_data, &send_mail_box);
}

/**
 * @brief 		速度模式发送函数
 * @param[in] id 帧 ID 为设定的 CAN ID 值加上 0X200的偏移
 * @param[in] _vel V_des：速度给定，浮点型，低位在前，高位在后
 * @retval 		none
 */
void ctrl_J4310motor_speed(uint16_t id, float _vel)
{
	uint8_t *vbuf;
	vbuf=(uint8_t*)&_vel;
	
	uint32_t send_mail_box;
	chassis_tx_message.StdId = id;
	chassis_tx_message.IDE = CAN_ID_STD;
	chassis_tx_message.RTR = CAN_RTR_DATA;
	chassis_tx_message.DLC = 0x08;
	chassis_can_send_data[0] = *vbuf;
	chassis_can_send_data[1] = *(vbuf+1);
	chassis_can_send_data[2] = *(vbuf+2);
	chassis_can_send_data[3] = *(vbuf+3);
	HAL_CAN_AddTxMessage(&hcan, &chassis_tx_message, chassis_can_send_data, &send_mail_box);
}	

/**
 * @brief 		位置速度模式发送函数
 * @param[in] id 帧 ID 为设定的 CAN ID 值加上 0X100的偏移
 * @param[in] _pos P_des：位置给定，浮点型，低位在前，高位在后
 * @param[in] _vel V_des：速度给定，浮点型，低位在前，高位在后
 * @retval 		none
 */
void ctrl_J4310motor_speed_pos(uint16_t id, float _pos, float _vel)
{
	uint8_t *pbuf,*vbuf;
	pbuf=(uint8_t*)&_pos;
	vbuf=(uint8_t*)&_vel;
	
	uint32_t send_mail_box;
	chassis_tx_message.StdId = id;
	chassis_tx_message.IDE = CAN_ID_STD;
	chassis_tx_message.RTR = CAN_RTR_DATA;
	chassis_tx_message.DLC = 0x08;
	chassis_can_send_data[0] = *pbuf;
	chassis_can_send_data[1] = *(pbuf+1);
	chassis_can_send_data[2] = *(pbuf+2);
	chassis_can_send_data[3] = *(pbuf+3);
	chassis_can_send_data[4] = *vbuf;
	chassis_can_send_data[5] = *(vbuf+1);
	chassis_can_send_data[6] = *(vbuf+2);
	chassis_can_send_data[7] = *(vbuf+3);
	HAL_CAN_AddTxMessage(&hcan, &chassis_tx_message, chassis_can_send_data, &send_mail_box);
}	

void ctrl_J4310motor_mit(uint16_t id, float _pos, float _vel, float _KP, float _KD, float _torq)
{
  uint16_t pos_tmp,vel_tmp,kp_tmp,kd_tmp,tor_tmp;
	pos_tmp = float_to_uint(_pos, P_MIN, P_MAX, 16);
	vel_tmp = float_to_uint(_vel, V_MIN, V_MAX, 12);
	kp_tmp = float_to_uint(_KP, KP_MIN, KP_MAX, 12);
	kd_tmp = float_to_uint(_KD, KD_MIN, KD_MAX, 12);
	tor_tmp = float_to_uint(_torq, T_MIN, T_MAX, 12);

  uint32_t send_mail_box;
  chassis_tx_message.StdId = id;
  chassis_tx_message.IDE = CAN_ID_STD;
  chassis_tx_message.RTR = CAN_RTR_DATA;
  chassis_tx_message.DLC = 0x08;
	chassis_can_send_data[0] = (pos_tmp >> 8);
	chassis_can_send_data[1] = pos_tmp;
	chassis_can_send_data[2] = (vel_tmp >> 4);
	chassis_can_send_data[3] = ((vel_tmp&0xF)<<4)|(kp_tmp>>8);
	chassis_can_send_data[4] = kp_tmp;
	chassis_can_send_data[5] = (kd_tmp >> 4);
	chassis_can_send_data[6] = ((kd_tmp&0xF)<<4)|(tor_tmp>>8);
	chassis_can_send_data[7] = tor_tmp;
  HAL_CAN_AddTxMessage(&hcan, &chassis_tx_message, chassis_can_send_data, &send_mail_box);
}	

/**
 * @brief          CAN2发送取矿电机控制电流(0x201,0x202)
 * @param[in]      sidesway		: (0x201) 横移电机, 2006电机控制电流范围 [-10000,10000]
 * @param[in]      sucker_roll: (0x202) 吸盘roll轴电机, 2006电机控制电流范围 [-10000,10000]
 * @param[in]      teles_ex_l	: (0x203) 左外伸缩电机，2006电机控制电流范围 [-10000,10000]
 * @param[in]      teles_ex_r	: (0x204) 右外伸缩电机，2006电机控制电流范围 [-10000,10000]
 * @retval         none
 */
void CAN_cmd_ore_extraction(int16_t sidesway, int16_t sucker_roll, int16_t teles_ex_l, int16_t teles_ex_r)
{
    uint32_t send_mail_box;
    gimbal_tx_message.StdId = CAN_ORE_EXTRACTION_ALL_ID;
    gimbal_tx_message.IDE = CAN_ID_STD;
    gimbal_tx_message.RTR = CAN_RTR_DATA;
    gimbal_tx_message.DLC = 0x08;
    gimbal_can_send_data[0] = (sidesway >> 8);
    gimbal_can_send_data[1] = sidesway;
    gimbal_can_send_data[2] = (sucker_roll >> 8);
    gimbal_can_send_data[3] = sucker_roll;
    gimbal_can_send_data[4] = (teles_ex_l >> 8);
    gimbal_can_send_data[5] = teles_ex_l;
    gimbal_can_send_data[6] = (teles_ex_r >> 8);
    gimbal_can_send_data[7] = teles_ex_r;
    HAL_CAN_AddTxMessage(&hcan, &gimbal_tx_message, gimbal_can_send_data, &send_mail_box);
}

/**
 * @brief          CAN2 发送救援电机控制电流(0x205, 0x206, 0x207, rev)
 * @param[in]      rescue_l: (0x205) 左救援勾电机, 2006电机控制电流范围 [-10000,10000]
 * @param[in]      rescue_r: (0x206) 右救援勾电机, 2006电机控制电流范围 [-10000,10000]
 * @param[in]      rev1		 : (0x207) 保留, 电机控制电流
 * @param[in]      rev2		 : (0x208) 保留, 电机控制电流
 * @retval         none
 */
void CAN_cmd_rescue(int16_t rescue_l, int16_t rescue_r, int16_t rev1, int16_t rev2)
{
    uint32_t send_mail_box;
    gimbal_tx_message.StdId = CAN_RESCUE_ALL_ID;
    gimbal_tx_message.IDE = CAN_ID_STD;
    gimbal_tx_message.RTR = CAN_RTR_DATA;
    gimbal_tx_message.DLC = 0x08;
    gimbal_can_send_data[0] = (rescue_l >> 8);
    gimbal_can_send_data[1] = rescue_l;
    gimbal_can_send_data[2] = (rescue_r >> 8);
    gimbal_can_send_data[3] = rescue_r;
    gimbal_can_send_data[4] = (rev1 >> 8);
    gimbal_can_send_data[5] = rev1;
    gimbal_can_send_data[6] = (rev2 >> 8);
    gimbal_can_send_data[7] = rev2;
    HAL_CAN_AddTxMessage(&hcan, &gimbal_tx_message, gimbal_can_send_data, &send_mail_box);
}

/**
  * @brief          send CAN packet of ID 0x700, it will set chassis motor 3508 to quick ID setting
  * @param[in]      none
  * @retval         none
  */
/**
  * @brief          发送ID为0x700的CAN包,它会设置3508电机进入快速设置ID
  * @param[in]      none
  * @retval         none
  */
void CAN_cmd_chassis_reset_ID(void)
{
  uint32_t send_mail_box;
  chassis_tx_message.StdId = 0x700;
  chassis_tx_message.IDE = CAN_ID_STD;
  chassis_tx_message.RTR = CAN_RTR_DATA;
  chassis_tx_message.DLC = 0x08;
  chassis_can_send_data[0] = 0;
  chassis_can_send_data[1] = 0;
  chassis_can_send_data[2] = 0;
  chassis_can_send_data[3] = 0;
  chassis_can_send_data[4] = 0;
  chassis_can_send_data[5] = 0;
  chassis_can_send_data[6] = 0;
  chassis_can_send_data[7] = 0;
  HAL_CAN_AddTxMessage(&hcan, &chassis_tx_message, chassis_can_send_data, &send_mail_box);
}

/**
 * @brief 发送给主控板的消息到CAN2
 * 
 * @param Info 
 */
void Publish_Info_To_CAN2(uint8_t Info[8])
{
    uint32_t send_mail_box;
    gimbal_tx_message.StdId = SLAVE_Info;
    gimbal_tx_message.IDE = CAN_ID_STD;
    gimbal_tx_message.RTR = CAN_RTR_DATA;
    gimbal_tx_message.DLC = 0x08;
    gimbal_can_send_data[0] = Info[0];
    gimbal_can_send_data[1] = Info[1];
    gimbal_can_send_data[2] = Info[2];
    gimbal_can_send_data[3] = Info[3];
    gimbal_can_send_data[4] = Info[4];
    gimbal_can_send_data[5] = Info[5];
    gimbal_can_send_data[6] = Info[6];
    gimbal_can_send_data[7] = Info[7];

    HAL_CAN_AddTxMessage(&hcan, &gimbal_tx_message, gimbal_can_send_data, &send_mail_box);
}

/**
 * @brief          return the sucker yaw 4310 motor data point
 * @param[in]      none
 * @retval         motor data point
 */
/**
 * @brief          返回吸盘yaw电机 4310电机数据指针
 * @param[in]      none
 * @retval         电机数据指针
*/
const dm_measure_t *get_sucker_yaw_measure_point(void)
{
  return &dm_chassis[0];
}

/**
 * @brief          return the sucker pitch 4310 motor data point
 * @param[in]      none
 * @retval         motor data point
 */
/**
 * @brief          返回吸盘前端pitch电机 4310电机数据指针
 * @param[in]      none
 * @retval         电机数据指针
*/
const dm_measure_t *get_sucker_pitch_F_measure_point(void)
{
  return &dm_chassis[1];
}

/**
 * @brief          return the sucker pitch 4310 motor data point
 * @param[in]      none
 * @retval         motor data point
 */
/**
 * @brief          返回吸盘末端pitch电机 4310电机数据指针
 * @param[in]      none
 * @retval         电机数据指针
*/
const dm_measure_t *get_sucker_pitch_B_measure_point(void)
{
  return &dm_chassis[2];
}

/**
 * @brief          return the gimbal 2006 motor data point
 * @param[in]      i: motor number,range [0,3]
 * @retval         motor data point
 */
/**
 * @brief          返回横移电机 2006电机数据指针
 * @param[in]      none
 * @retval         电机数据指针
*/
const motor_measure_t *get_sidesway_motor_measure_point(void)
{
  return &motor_gimbal[0];
}

/**
 * @brief          return the gimbal 2006 motor data point
 * @param[in]      i: motor number,range [0,3]
 * @retval         motor data point
 */
/**
 * @brief          返回吸盘roll轴电机 2006电机数据指针
 * @param[in]      none
 * @retval         电机数据指针
*/
const motor_measure_t *get_sucker_roll_motor_measure_point(void)
{
  return &motor_gimbal[1];
}

/**
 * @brief          return the telescope_ex_l 2006 motor data point
 * @param[in]      none
 * @retval         motor data point
 */
/**
 * @brief          返回左外伸缩电机 2006电机数据指针
 * @param[in]      none
 * @retval         电机数据指针
*/
const motor_measure_t *get_telescope_ex_l_motor_measure_point(void)
{
  return &motor_gimbal[2];
}

/**
 * @brief          return the telescope_ex_ 2006 motor data point
 * @param[in]      none
 * @retval         motor data point
 */
/**
 * @brief          返回右外伸缩电机 2006电机数据指针
 * @param[in]      none
 * @retval         电机数据指针
*/
const motor_measure_t *get_telescope_ex_r_motor_measure_point(void)
{
  return &motor_gimbal[3];
}

/**
 * @brief          return the rescue_l 2006 motor data point
 * @param[in]      none
 * @retval         motor data point
 */
/**
 * @brief          返回救援勾左电机 2006电机数据指针
 * @param[in]      none
 * @retval         电机数据指针
*/
const motor_measure_t *get_rescue_l_motor_measure_point(void)
{
  return &motor_gimbal[4];
}

/**
 * @brief          return the rescue_l 2006 motor data point
 * @param[in]      none
 * @retval         motor data point
 */
/**
 * @brief          返回救援勾右电机 2006电机数据指针
 * @param[in]      none
 * @retval         电机数据指针
*/
const motor_measure_t *get_rescue_r_motor_measure_point(void)
{
  return &motor_gimbal[5];
}

